package ext.sim.contact.detect;

import ext.sim.Body;
import ext.sim.BodyCuboid;
import ext.sim.BodySphere;
import ext.sim.contact.Contact;
import ext.sim.contact.ContactPair;
import java.util.ArrayList;
import java.util.Collection;
import java.util.LinkedList;
import math.region.Sphere3;
import math.vec3;
import static math.vec3.*;

public class ContactDetector
{
    
    
    private LinkedList<ContactPair> mCandidatePairs;
    
    private LinkedList<Contact> mContacts;
    
    
    public ContactDetector()
    {
        this.mCandidatePairs = new LinkedList<>();
        this.mContacts = new LinkedList<>();
    }
    
    
    public LinkedList<Contact> detect(ArrayList<Body> aBodies)
    {
        // BROAD-PHASE CONTACT DETECTION: // TODO: ADD A SUBDIVISION SCHEME TO AVOID ALL-PAIRS.
        this.detectBroadPhase(aBodies, this.mCandidatePairs);
        
        // NARROW-PHASE CONTACT DETECTION: // TODO: OPTIMIZE PAIR-WISE DETECTION!
        this.detectNarrowPhase(this.mCandidatePairs, this.mContacts);
        
        return this.mContacts;
    }
    
    
    
    private void detectBroadPhase(
            final ArrayList<Body> aBodies, 
            LinkedList<ContactPair> aCandidatePairs)
    {
        aCandidatePairs.clear();
        
        int N = aBodies.size();
        for (int I = 0;     I < N - 1; I++)
        for (int J = I + 1; J < N;     J++)
        {
            Body A = aBodies.get(I);
            Body B = aBodies.get(J);
            
            if (A.getBoundingSphere().overlaps(B.getBoundingSphere()))
            {
                aCandidatePairs.add(new ContactPair(A, B, I, J));
            }
        }
    }
    
    private void detectNarrowPhase(
            final LinkedList<ContactPair> aCandidates, 
            LinkedList<Contact> aContacts)
    {
        aContacts.clear();
        
        for (ContactPair pair : aCandidates)
        {
            Body A = pair.mA;
            Body B = pair.mB;
            
            int I = pair.mI;
            int J = pair.mJ;
            
            boolean Acuboid = A instanceof BodyCuboid;
            boolean Bcuboid = B instanceof BodyCuboid;
            
            if (Acuboid && Bcuboid) // CUBOID-CUBOID DETECTION.
            {
                BodyCuboid Ac = (BodyCuboid) A;
                BodyCuboid Bc = (BodyCuboid) B;
                
                Collection<Sphere3> candidA = new LinkedList<>();
                Collection<Sphere3> candidB = new LinkedList<>();
                Ac.overlapCandidates(Bc, candidA, candidB);
                
                for (Sphere3 sA : candidA)
                for (Sphere3 sB : candidB)
                {
                    Contact contact = this.contactSphereSphere(Ac, Bc, I, J, sA, sB);
                    if (contact != null)
                    {
                        aContacts.addLast(contact);
                    }
                }
            }
            else if (Acuboid) // CUBOID-SPHERE DETECTION.
            {
                BodyCuboid Ac = (BodyCuboid) A;
                BodySphere Bs = (BodySphere) B;
                
                Collection<Sphere3> candidA = new LinkedList<>();
                Collection<Sphere3> candidB = new LinkedList<>(); // Will have length = 1.
                Ac.overlapCandidates(Bs, candidA, candidB);
                
                Sphere3 sB = B.getBoundingSphere();
                for (Sphere3 sA : candidA)
                {
                    Contact contact = this.contactSphereSphere(Ac, Bs, I, J, sA, sB);
                    if (contact != null)
                    {
                        aContacts.addLast(contact);
                    }
                }
            }
            else if (Bcuboid) // SPHERE-CUBOID DETECTION.
            {
                BodySphere As = (BodySphere) A;
                BodyCuboid Bc = (BodyCuboid) B;
                
                Collection<Sphere3> candidA = new LinkedList<>(); // Will have length = 1.
                Collection<Sphere3> candidB = new LinkedList<>();
                Bc.overlapCandidates(As, candidA, candidB);
                
                Sphere3 sA = A.getBoundingSphere();
                for (Sphere3 sB : candidB)
                {
                    Contact contact = this.contactSphereSphere(As, Bc, I, J, sA, sB);
                    if (contact != null)
                    {
                        aContacts.addLast(contact);
                    }
                }
            }
            else // SPHERE-SPHERE DETECTION.
            {
                BodySphere As = (BodySphere) A;
                BodySphere Bs = (BodySphere) B;
                
                Contact contact = this.contactSphereSphere(As, Bs, I, J, As.getBoundingSphere(), Bs.getBoundingSphere());
                if (contact != null)
                {
                    aContacts.addLast(contact);
                }
            }
        }
    }
    
    
    private Contact contactSphereSphere(
            final Body A, final Body B, 
            int I, int J, 
            final Sphere3 sA, final Sphere3 sB)
    {
        double sumRad = sA.mRadius + sB.mRadius;
        double sqSumRad = sumRad * sumRad;
        vec3 BsubA = sub(sB.mOrig, sA.mOrig);
        if (lengthSq(BsubA) > sqSumRad)
        {
            return null;
        }
        vec3 point = mul(BsubA, 0.5);
        point.add(sA.mOrig);
        
        vec3 normal = norm(BsubA);
        return new Contact(A, B, I, J, point, normal);
    }
    
    
}
